#include <NewLifeDDK.h>
#include <string.h>
#include "kbdrv.h"
#include "kbdus.h"

CKeyboardDriver::CKeyboardDriver()
{
	// Prepare keyboard buffer
	keyboard_buffer = new unsigned char[KEYBOARD_BUFFER_SIZE];
	memset(keyboard_buffer, 0, KEYBOARD_BUFFER_SIZE);
	keyboard_buffer_in = keyboard_buffer_out = 0;

	ctrl_alt_del_enabled = 1;
}

u32 CKeyboardDriver::InitializeDriver()
{
	// Initialize keyboard tables
	keytables[0] = &uskeys;
	keymap = 0;

	return reset_keyboard();
}

//
// Reset keyboard hardware
//
int CKeyboardDriver::reset_keyboard()
{
	int status;

	// Test the keyboard interface
	kbd_wait();
	kbd_write_command(KBD_CCMD_SELF_TEST);
	if (kbd_wait_for_data() != 0x55)
	{
		return -1;
	}
	
	// Perform a keyboard interface test. This causes the controller
	// to test the keyboard clock and data lines.
	kbd_wait();
	kbd_write_command(KBD_CCMD_KBD_TEST);
	if (kbd_wait_for_data() != 0x00)
	{
		return -1;
	}

	// Enable the keyboard by allowing the keyboard clock to run
	kbd_wait();
	kbd_write_command(KBD_CCMD_KBD_ENABLE);

	// Reset keyboard. If the read times out then the assumption is that no
	// keyboard is plugged into the machine.
	while (1)
	{
		kbd_wait();
		kbd_write_data(KBD_CMD_RESET);
		status = kbd_wait_for_data();
		if (status == KBD_REPLY_ACK) break;
		if (status != KBD_REPLY_RESEND)
		{
			return -1;
		}
	}

	if ((status = kbd_wait_for_data()) != KBD_REPLY_POR)
	{
		return -1;
	}

	// Set keyboard controller mode. During this, the keyboard should be
	// in the disabled state.
	while (1)
	{
		kbd_wait();
		kbd_write_data(KBD_CMD_DISABLE);
		status = kbd_wait_for_data();
		if (status == KBD_REPLY_ACK) break;
		if (status != KBD_REPLY_RESEND)
		{
			return -1;
		}
	}

	kbd_wait();
	kbd_write_command(KBD_CCMD_WRITE_MODE);

	kbd_wait();
	kbd_write_data(KBD_MODE_KBD_INT | KBD_MODE_SYS | KBD_MODE_DISABLE_MOUSE | KBD_MODE_KCC);

	kbd_wait();
	kbd_write_data(KBD_CMD_ENABLE);
	if (kbd_wait_for_data() != KBD_REPLY_ACK)
	{
		return -1;
	}

	// Set the typematic rate to maximum
	kbd_wait();
	kbd_write_data(KBD_CMD_SET_RATE);
	if (kbd_wait_for_data() != KBD_REPLY_ACK)
	{
		return -1;
	}
	
	kbd_wait();
	kbd_write_data(0x00);
	if (kbd_wait_for_data() != KBD_REPLY_ACK)
	{
		return -1;
	}

	// Set all keyboard LEDs off
	led_status = 0; 
	setleds();

	return 0;
}

//
// Get one character from keyboard
//
int CKeyboardDriver::getch(unsigned int timeout)
{
	unsigned char ch;

	ch = keyboard_buffer[keyboard_buffer_out];
	keyboard_buffer_out = (keyboard_buffer_out + 1) & (KEYBOARD_BUFFER_SIZE - 1);

	return ch;
}

//
// Test for any keyboard input.
// Returns 0 if keyboard buffer is empty, else returns 1.
//
int CKeyboardDriver::kbhit()
{
	return keyboard_buffer_in != keyboard_buffer_out;
}

void CKeyboardDriver::irq_handler(u16 irq)
{
	unsigned int scancode;
	unsigned char status;

	while ((status = _inp(KBD_STATUS)) & KBD_STAT_OBF) 
	{
		// Get next scancode from keyboard
		scancode = _inp(KBD_DATA) & 0xFF;
		// Process scan code
		process_scancode(scancode);
	}
}

//
// Process keyboard scancode
//

void CKeyboardDriver::process_scancode(unsigned int scancode)
{
	unsigned int keycode = 0;
	struct keytable *kt;
	int state;

	// In scancode mode just insert scancode
	if (keymap == -1)
	{
		insert_key((unsigned char) scancode);
		return;
	}

	// Get keytable
	kt = keytables[keymap];
	if (!kt) return;

	// Extended scancode
	if (scancode == 0xE0)
	{
		ext = 1;
		return;
	}

	// Ctrl-Alt-SysRq
	if ((control_keys & (CK_LCTRL | CK_LALT)) && scancode == 0xD4) 
	{
//		dbg_break();
		return;
	}

	// Ctrl-Alt-Del
	if ((control_keys & (CK_LCTRL | CK_LALT)) && scancode == 0x53) 
	{
		if (ctrl_alt_del_enabled) reboot();
	}

	// LED keys, i.e. scroll lock, num lock, and caps lock
	if (scancode == 0x3A)
	{
		// Caps lock
		led_status ^= LED_CAPS_LOCK;
		setleds();
	}

	if (scancode == 0x45)
	{
		// Num lock
		led_status ^= LED_NUM_LOCK;
		setleds();
	}

	if (scancode == 0x46)
	{
		// Scroll lock
		led_status ^= LED_SCROLL_LOCK;
		setleds();
	}

	// Ctrl keys
	if (!ext && scancode == 0x1D) control_keys |= CK_LCTRL;
	if (!ext && scancode == (0x1D | 0x80)) control_keys &= ~CK_LCTRL;

	if (ext && scancode == 0x1D) control_keys |= CK_RCTRL;
	if (ext && scancode == (0x1D | 0x80)) control_keys &= ~CK_RCTRL;

	// Shift keys
	if (scancode == 0x2A) control_keys |= CK_LSHIFT;
	if (scancode == (0x2A | 0x80)) control_keys &= ~CK_LSHIFT;

	if (scancode == 0x36) control_keys |= CK_RSHIFT;
	if (scancode == (0x36 | 0x80)) control_keys &= ~CK_RSHIFT;

	// Alt key
	if (!ext && scancode == 0x38) control_keys |= CK_LALT;
	if (!ext && scancode == 0x80 + 0x38) control_keys &= ~CK_LALT;
					
	// AltGr key
	if (ext && scancode == 0x38) control_keys |= CK_RALT;
	if (ext && scancode == (0x38 | 0x80)) control_keys &= ~CK_RALT;

	if (scancode < MAX_SCANCODES)
	{
		if ((control_keys & (CK_LSHIFT | CK_RSHIFT)) && (led_status & LED_CAPS_LOCK))
			state = KBSTATE_SHIFTCAPS;
		else if (control_keys & (CK_LSHIFT | CK_RSHIFT))
			state = KBSTATE_SHIFT;
		else if (control_keys & (CK_LCTRL | CK_RCTRL))
			state = KBSTATE_CTRL;
		else if (control_keys & CK_LALT) 
			state = KBSTATE_ALT;
		else if (control_keys & CK_RALT)
			state = KBSTATE_ALTGR;
		else if ((control_keys & (CK_LSHIFT | CK_RSHIFT)) && (led_status & LED_NUM_LOCK))
			state = KBSTATE_SHIFTNUM;
		else if (led_status & LED_CAPS_LOCK) 
			state = KBSTATE_CAPSLOCK;
		else if (led_status & LED_NUM_LOCK) 
			state = KBSTATE_NUMLOCK;
		else if (control_keys == 0) 
			state = KBSTATE_NORMAL;

		//kprintf("(%d,%x)", state, control_keys);

		if (ext)
			keycode = kt->extended[scancode][state];
		else
			keycode = kt->normal[scancode][state];

		if (keycode != 0)
		{
			if (keycode <= 0xFF)
			{
				insert_key((unsigned char) keycode);
			}
			else
			{
				insert_key((unsigned char) (keycode & 0xFF));
				insert_key((unsigned char) (keycode >> 8));
			}
		}
	}

	ext = 0;
}

//
// Wait for keyboard ready
//

void CKeyboardDriver::kbd_wait()
{
	int tmo = KEYBOARD_TIMEOUT;

	while (_inp(KBD_STATUS) & KBD_STAT_IBF)
	{
		if (--tmo == 0)
		{
//			kprintf("kbd: busy timeout\n");
			return;
		}

//		udelay(1);
	}
}

//
// Read data from keyboard
//

int CKeyboardDriver::kbd_read_data()
{
	unsigned char status;

	status = _inp(KBD_STATUS);
	if (status & KBD_STAT_OBF) 
	{
		unsigned char data = _inp(KBD_DATA);

		if (status & (KBD_STAT_GTO | KBD_STAT_PERR)) 
		return -1;
		//	return -EIO;
		
		else
			return data;
	}
	else
		return -2;
		//return -EAGAIN;
}

//
// Wait for data from keyboard
//

int CKeyboardDriver::kbd_wait_for_data()
{
	int tmo = KEYBOARD_TIMEOUT;

	while (1)
	{
		int rc = kbd_read_data();
		if (rc >= 0) return rc;

		if (--tmo == 0)
		{
			//kprintf("kbd: read timeout\n");
			return -1;//ETIMEOUT;
		}

//		udelay(1);
	}
}

//
// Write data to keyboard
//

void CKeyboardDriver::kbd_write_data(unsigned char data)
{
	_outp(KBD_DATA, data);
}

//
// Write command to keyboard
//

void CKeyboardDriver::kbd_write_command(unsigned char cmd)
{
	_outp(KBD_COMMAND, cmd);
}

//
// Reboot machine
//

void CKeyboardDriver::reboot()
{
	kbd_wait();
	kbd_write_command(0xFE);
	__asm__("a: jmp a");
}							 

//
// Set keyboard LEDs
//

void CKeyboardDriver::setleds()
{
	kbd_write_data(KBD_CMD_SET_LEDS);
	kbd_wait();
	kbd_write_data(led_status);
	kbd_wait();
}

//
// Insert into keyboard buffer
//

void CKeyboardDriver::insert_key(unsigned char ch)
{
	if (((keyboard_buffer_in + 1) & (KEYBOARD_BUFFER_SIZE - 1)) != keyboard_buffer_out)
	{
		keyboard_buffer[keyboard_buffer_in] = ch;
		keyboard_buffer_in = (keyboard_buffer_in + 1) & (KEYBOARD_BUFFER_SIZE - 1);
	}
}
